#include "xServo.h"



///**
//     * @brief 获取Sdo信息
//     * @param  slave            从站地址 1~n
//     * @param  addr             EtherCAT SDO地址
//     * @param  size             数据大小
//     * @param  data             数据
//     */
//static void readSdo(int slave, int addr, int &size, uint8_t *data);

xServo::xServo() {}

void xServo::initPdoOutput()
{
    servo_output = (ECatDrv::xServoPDOOutput*)ec_slave[slave_addr].outputs;
}

void xServo::initPdoInput()
{
    servo_input = (ECatDrv::xServoPDOInput*)ec_slave[slave_addr].inputs;
}

xServo::xServo(unsigned int eep_man, unsigned int eep_id, const std::string &slave_name, int slave_addr,
               ECatDrv::slaveType slave_type)
    :ECatDrv::ServoDrive(eep_man, eep_id, slave_name, slave_addr, slave_type)
{}


int xServo::configurePDO()
{
    int retval = 0;
    int indexSize;
    int res = 0;
    // 伺服反馈信号
    uint32_t txPdoGroup[]=
        {
            0x60640020,         //postion fb
            //        0x606C0020,         //velocity fb
            //        0x24020010,         //ai2
            //        0x60770010,         //torque fb
            //        0x60F40020,         //follow error
            //        0x24010010,         //ai1
            //        0x60410010,         //status word
            //        0x60610008,         //mode of operation display
        };

    // 发到伺服的指令信号
    uint32_t rxPdoGroup[]=
        {
            0x20010008,
            //        0x60FF0020,     //target velocity
            //        0x60600008,     //mode of opration
            //        0x60B20010,     //torque offset
            //        0x60B10020,     //velocity offset
            //        0x60400010,     //control word
            //        0x60710010,     //target torque
            //        0x607A0020,     //target postion
        };


    // 写1c12和1c13时 CA 需要设置为true
    retval += ec_SDOwrite(slave_addr, 0x1c12, 0x00, TRUE, 2, &res, EC_TIMEOUTSAFE);
    retval += ec_SDOwrite(slave_addr, 0x1c13, 0x00, TRUE, 2, &res, EC_TIMEOUTSAFE);

#if 0

    // ec_SDOwrite 的 CA 位 为TRUE时会卡死
    // ec_SDOwrite 配置对象字典时先配置subIndex=1， 再配置 subindex=0
    indexSize = sizeof(txPdoGroup)/sizeof(uint32_t);
    for(int i=0;i<indexSize;i++)
    {
        retval += ec_SDOwrite(slave_addr, 0x1A00, i+1, FALSE, 4, &txPdoGroup[i], EC_TIMEOUTSAFE*4);
    }
    retval += ec_SDOwrite(slave_addr, 0x1A00, 0x00, FALSE, 2, &indexSize, EC_TIMEOUTSAFE*4);

    indexSize = sizeof(rxPdoGroup)/sizeof(uint32_t);

    for(int i=0;i<indexSize;i++)
    {
        retval += ec_SDOwrite(slave_addr, 0x1600, i+1, FALSE, 4, &rxPdoGroup[i], EC_TIMEOUTSAFE*4);
    }
    retval += ec_SDOwrite(slave_addr, 0x1600, 0x00, FALSE, 2, &indexSize, EC_TIMEOUTSAFE*4);
#else
    uint32 map_1c12[] = {0x16000001};
    uint32 map_1c13[] = {0x1A000001};
    // 写1c12和1c13时 CA 需要设置为true
    retval += ec_SDOwrite(slave_addr, 0x1c12, 0x00, TRUE, sizeof (map_1c12), &map_1c12, EC_TIMEOUTSAFE*4);
    retval += ec_SDOwrite(slave_addr, 0x1c13, 0x00, TRUE, sizeof (map_1c13), &map_1c13, EC_TIMEOUTSAFE*4);
#endif
    while(EcatError)
    {
        printf("%s", ec_elist2string());
        //       qDebug()<< ec_elist2string();
    }
    return 1;
}

#ifdef xServo_SDO_CONTROL
bool xServo::ServoEnable()
{
    int dir = 1;
    servo_win_frame_s frame_send,frame_rev;
    uint16_t head = 0;
    frame_send.head = head;
    frame_send.len = 1;
    frame_send.func = 0x61;
    frame_send.buf[0] = dir;
    //    frame_send.buf[1] = vol;

    CommuFrame((uint16_t *)&frame_send,(uint16_t *)&frame_rev);

    qDebug()<<"ServoOn";
    return true;

}

void xServo::ServoDisable()
{
    int dir = 0;
    servo_win_frame_s frame_send,frame_rev;
    uint16_t head = 0;
    frame_send.head = head;
    frame_send.len = 1;
    frame_send.func = 0x61;
    frame_send.buf[0] = dir;
    //    frame_send.buf[1] = vol;

    CommuFrame((uint16_t *)&frame_send,(uint16_t *)&frame_rev);

    qDebug()<<"ServoOff";
}
#endif

bool xServo::ServoJog(int dir, uint16_t head, int vel)
{
    servo_win_frame_s frame_send, frame_rev;

    if(vel<0) return false;
    if(vel>=4000) vel = 4000;

    frame_send.head = head;
    frame_send.len = 2;
    frame_send.func = 0x62;
    frame_send.buf[0] = dir;
    frame_send.buf[1] = vel;
    CommuFrame((uint16_t *)&frame_send,(uint16_t *)&frame_rev);

    return true;
}

#ifndef xServo_SDO_CONTROL
void xServo::StMachineUpdate()
{
    GetSlaveSdo(CIA_STATE_WORD, 2, (unsigned int*)&m_state.stateVal);
    GetSlaveSdo(CIA_ERROR_CODE, 2, (unsigned int*)&m_state.errorCode);
    GetSlaveSdo(CIA_OP_MODE_DISPLAY, 1, (unsigned int*)&m_state.opVal);

    uint16_t status = m_state.stateVal;
    uint16_t error = m_state.errorCode;

    if (status & 0x08)
    {
        m_state.ctrlMode = ECatDrv::ServoControlMode::Fault;
    }
    else if ((status & 0x004F) == 0x0000)
    {
        m_state.ctrlMode = ECatDrv::ServoControlMode::NotReadyToSwicthOn;
    }
    else if ((status & 0x004F) == 0x0040)
    {
        m_state.ctrlMode = ECatDrv::ServoControlMode::SwitchOnDisabled;
    }
    else if ((status & 0x006F) == 0x0021)
    {
        m_state.ctrlMode = ECatDrv::ServoControlMode::ReadyToSwitchOn;
    }
    else if ((status & 0x006F) == 0x0023)
    {
        m_state.ctrlMode = ECatDrv::ServoControlMode::SwitchOn;
    }
    else if ((status & 0x006F) == 0x0027)
    {
        m_state.ctrlMode = ECatDrv::ServoControlMode::OperationEnable;
    }
    else if ((status & 0x006F) == 0x0007)
    {
        m_state.ctrlMode = ECatDrv::ServoControlMode::QuickStopActive;
    }

    if (!m_control.ServoEnable)
    {
        // 用户未请求上电，可加入下电逻辑
        if (m_state.ctrlMode == ECatDrv::ServoControlMode::OperationEnable)
        {
            servo_output->controlword = 0x0006;
        }
    }

    switch (m_state.ctrlMode) {
        case ECatDrv::ServoControlMode::Fault:
            // 伺服在故障状态，执行 Fault Reset
                servo_output->controlword = 0x0080;  // Fault reset
//                qDebug() << "Fault Reset slave id:" << slave_addr;
            break;
        case ECatDrv::ServoControlMode::SwitchOnDisabled:
            // 发 Shutdown 使能
            if (m_control.ServoEnable)
            {
                servo_output->controlword = 0x0006;
//                qDebug() << "SwitchOnDisabled slave id:" << slave_addr;
            }
            break;
        case ECatDrv::ServoControlMode::ReadyToSwitchOn:
            // 发 Switch On
            if (m_control.ServoEnable)
            {
                servo_output->controlword = 0x0007;
//                qDebug() << "ReadyToSwitchOn slave id:" << slave_addr;
            }
            break;
        case ECatDrv::ServoControlMode::SwitchOn:
            // 发 Enable Operation
            if (m_control.ServoEnable)
            {
                servo_output->controlword = 0x000F;
//                qDebug() << "SwitchOn slave id:" << slave_addr;
            }
            break;
        case ECatDrv::ServoControlMode::OperationEnable:
            // 到达目标状态
            // 可加入标志位，表示"伺服已上电"
            break;
        case ECatDrv::ServoControlMode::NotReadyToSwicthOn:
//            qDebug() << "[Servo] Not ready to switch on.";
            break;
        case ECatDrv::ServoControlMode::QuickStopActive:
//            qDebug() << "[Servo] In Quick Stop Active.";
            break;
        }

        // 4. 发送过程数据
        ec_send_processdata();
        ec_receive_processdata(EC_TIMEOUTRET);
}
#else
void xServo::StMachineUpdate()
{
    m_state.stateVal = servo_input->statusword;
    m_state.errorCode = servo_input->error_code;
    m_state.opVal = servo_input->modes_of_operation_display;
    m_state.velocity = servo_input->velocity_actual_value;

//    qDebug() << "state: " << QString("0x%1").arg(m_state.stateVal, 4, 16, QChar('0'));
//    qDebug() << "error code: " << m_state.errorCode;
//    qDebug() << "opVal: " << m_state.opVal;
//    qDebug() << "velocity_actual_value: " << -m_state.velocity;
//    qDebug() << "position_actual_value: " << servo_input->position_actual_value;
//    qDebug() << "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<";
}
#endif

void xServo::CommuFrame(uint16_t *send, uint16_t *recv)
{
    int size = 64;
    SetSlaveSdo(0x2200, size, (unsigned int*)send);
    GetSlaveSdo(0x2200, size, (unsigned int*)recv);
}

void xServo::JogOn(int vel)
{
    if (vel > 4000)
    {
        vel = 4000;
    }
    if (vel < -4000)
    {
        vel = -4000;
    }
#ifdef xServo_SDO_CONTROL
    if (vel > 0)
    {
        ServoJog(1, 0, vel);
    }
    else
    {
        ServoJog(0, 0, abs(vel));
    }
#else
    servo_output->modes_of_operation = ECatDrv::ServoOpMode::OP_CSV;
    ec_send_processdata();
    ec_receive_processdata(EC_TIMEOUTRET);
    QThread::msleep(100);
//    qDebug() << servo_output->modes_of_operation;

//    servo_output->controlword = 0x000f;
    servo_output->target_velocity = -vel;

//    servo_output->target_velocity = 0;
#endif
}

void xServo::JogOff()
{
#ifdef xServo_SDO_CONTROL
    ServoJog(1, 0, 0);
#else
    servo_output->target_velocity = 0;

    ec_send_processdata();
    ec_receive_processdata(EC_TIMEOUTRET);
#endif
}
